#include <tf/transform_datatypes.h>

#include <cmath>

int main()
{
    tf::Quaternion q1;
    q1.setRPY(0,0,M_PI_2);
    std::cout << q1.x() << ", " << q1.y() << ", " << q1.z() << ", " << q1.w() << "\n";
    std::cout << "q1.length: " << q1.length() << "\n";
    tf::Vector3 a1(1, 1, 0);
    tf::Vector3 axis = q1.getAxis();
    double angle = q1.getAngle();
    std::cout << "axis of q1 is: " << axis.x() << ", " << axis.y() << ", " << axis.z() << "\n";
    std::cout << "angle is: " << angle << "\n";
    tf::Vector3 a2 = a1.rotate(q1.getAxis(), q1.getAngle()); //计算原始点旋转之后的坐标
    std::cout<<"(1,1,0) after rotate with Z 90: "<<a2.x()<<","<<a2.y()<<","<<a2.z()<<std::endl;
    
    tf::Matrix3x3 rm(q1); // rotate matrix
    tf::Vector3 a22 = rm * a1;
    std::cout<<"(1,1,0) rotate with Z 90 in matrix: "<<a22.x()<<","<<a22.y()<<","<<a22.z()<<std::endl;
    return 0;
}